
#include "prog_manual_control.h"

/**
 * can-ctrl
 * Program: prog_manual_control
 */

uint32_t prog_manual_control_lastcheck = 0;
uint8_t prog_manual_control_turning = 0;
uint16_t prog_manual_control_servovalues[2] = {0,0};
uint8_t prog_manual_control_headlights = 0;

int8_t prog_manual_control_init(void)
{
	register_slot(prog_manual_control_buttonpressed, btn_event);
	register_slot(prog_manual_control_potchange,pot_event);
	lcd_clear_display();
	prog_manual_control_updatelcd();
	return 1;
}
int8_t prog_manual_control_uninit(void)
{
	unregister_slot(prog_manual_control_buttonpressed, btn_event);
	unregister_slot(prog_manual_control_potchange,pot_event);
	lcd_clear_display();

	return 1;
}
int8_t prog_manual_control_run(void)
{
	if(time() - prog_manual_control_lastcheck > 2000)
	{
		if(prog_manual_control_turning)
		{
			sound(200,100);
		}
		
		can_message_directionlights(
			prog_manual_control_turning ? prog_manual_control_turning : 0
									);
		
		
		prog_manual_control_lastcheck = time();
	}
	return 1;
	
}

void prog_manual_control_buttonpressed(Event *ev)
{
	Button *in_data = (Button *) ev->data;
	switch(*in_data)
	{
		case joy_up:
			can_message_headlights(1);
			prog_manual_control_headlights = 1;
			break;
		case joy_down:
			can_message_headlights(0);
			prog_manual_control_headlights = 0;
			break;
		case joy_left:
			
			if(prog_manual_control_turning)
				prog_manual_control_turning = 0;
			else
				prog_manual_control_turning = 1;
			
			break;
		case joy_right:
			if(prog_manual_control_turning)
				prog_manual_control_turning = 0;
			else
				prog_manual_control_turning = 2;
			break;
		case joy_enter:
			if(prog_manual_control_turning <3)
				prog_manual_control_turning = 3;
			else if(prog_manual_control_turning == 3)
				prog_manual_control_turning = 0;
			break;
		default:
			break;
	}
	prog_manual_control_updatelcd();
}

void prog_manual_control_potchange(Event *ev)
{
	Potentiometer *pot = (Potentiometer *) ev->data;
	uint8_t *value = (uint8_t *) (ev->data +1);

 	if(*pot == pota)
	{
		can_message_servo_position(1, (uint16_t) *value * 9);
		prog_manual_control_servovalues[pota] = (uint16_t)*value*9;
	}
	if(*pot == potb)
	{		
		can_message_servo_position(2, (uint16_t) *value * 9);
		prog_manual_control_servovalues[potb] = (uint16_t)*value*9;
	}
	prog_manual_control_updatelcd();
}

void prog_manual_control_updatelcd(void)
{
	lcd_write_P(0, PSTR("MANUAL CONTROL ENABLED"));
	lcd_write_P(2, PSTR("LIGHTS "));
	if(prog_manual_control_headlights)
		lcd_write_P(2, PSTR("ON "));
	else
		lcd_write_P(2, PSTR("OFF"));
	
	
	if(prog_manual_control_turning == 3)
		lcd_write_P(3, PSTR(" EMERGENCY  "));
	else if(prog_manual_control_turning==0)
		lcd_write_P(3, PSTR("                  "));
	else
	{
		lcd_write_P(3, PSTR("SIGNALLING "));
		if(prog_manual_control_turning==1)
			lcd_write_P(3, PSTR("LEFT"));
		else
			lcd_write_P(3, PSTR("RIGHT"));
	}
	
	char txt[6];
	lcd_write_P(5, PSTR("SERVO A:"));
	sprintf(txt, "%i ", (unsigned int) prog_manual_control_servovalues[pota]);
	lcd_write(5, txt);
	lcd_write_P(5, PSTR(" B:"));
	sprintf(txt, "%i", (unsigned int) prog_manual_control_servovalues[potb]);
	lcd_write(5, txt);
}